#define servo_num 4
#define servo_step_delay 15
#define servo_action_delay_time 500

float servel_parts = 100.0;        //设定舵机舵机份数
int value_init_int[servo_num] = {62, 139, 159, 19};//舵机初始角度int类型
float value_init_float[servo_num] = {62, 139, 159, 19};//舵机初始角度


//舵机引脚初始化函数
void servo_init(){
  servo.init();
  servo.move(value_init_int[0], value_init_int[0], value_init_int[0], value_init_int[0]);
  delay(3000);
}

//多个舵机同时动作函数
void servo_move(float value0, float value1, float value2, float value3, int time_action_delay){
  float value_arguments[] = {value0, value1, value2, value3};
  float value_delta[servo_num];

  //Serial.println("---------------------------------------------------------");
  for(int i=0;i<servo_num;i++){
    value_delta[i] = (value_arguments[i] - value_init_float[i]) / servel_parts;
    //Serial.print(value_delta[i]);Serial.print(" ");
  }
  //Serial.println();

  for(int i=0;i<servel_parts;i++){
    for(int k=0;k<servo_num;k++){
      value_init_float[k] = value_delta[k] == 0 ? value_arguments[k] : value_init_float[k] + value_delta[k];
      value_init_int[k] = int(value_init_float[k]);
      //Serial.print(value_init_int[k]);Serial.print(" ");
    }
    //Serial.println();

    servo.move(value_init_int[0], value_init_int[1], value_init_int[2], value_init_int[3]);
    delay(servo_step_delay);
  }
  delay(time_action_delay);
}

//舵机动作测试
void servo_action_test(){
  servo_move(62, 139, 159, 19, 1500); //机械臂初始化动作
  servo_move(62, 73, 159, 19, 1500);  //机械臂识别颜色动作
}